#include<ros/ros.h>
#include<geometry_msgs/Twist.h>
#include<can_msgs/TX2_VCU_MotorCmd.h>
#include<iostream>

double length_robot = 2.4;
double width_robot = 2.2;
double wheel_radius = 0.1;
double r1=0;

class Cmd_Drive{
public:
   Cmd_Drive();
private:
   void callBack(const geometry_msgs::Twist::ConstPtr& twist);
   ros::NodeHandle n;
   ros::Publisher pub;
   ros::Subscriber sub;
};

Cmd_Drive::Cmd_Drive()
{
   pub = n.advertise<can_msgs::TX2_VCU_MotorCmd>("can_msgs/TX2_VCU_MotorCmd",10);
   sub = n.subscribe<geometry_msgs::Twist>("cmd_vel", 10, &Cmd_Drive::callBack,this);
}

void Cmd_Drive::callBack(const geometry_msgs::Twist::ConstPtr& twist)
{   
    can_msgs::TX2_VCU_MotorCmd motor;
    
    double v = twist->linear.x;
    double w = twist->angular.z;
    r1 = v/w-width_robot/2;
    motor.LeftSteerPosition = std::atan(length_robot/r1);
    motor.RightSteerPosition = std::atan(length_robot/(r1+width_robot));

    motor.LeftDriveSpeed = (w*sqrt(length_robot*length_robot+r1*r1))/wheel_radius;
    motor.RightDriveSpeed = (w*sqrt(length_robot*length_robot+pow((r1+width_robot),2)))/wheel_radius; 
    pub.publish(motor); 
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "cmd_to_wheel");
    Cmd_Drive cmd_drive;
    ros::spin();

}


